/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : system.cpp
  * Description        : 
  ******************************************************************************
  * Function           : 创建任务、信号量、消息队列、软件定时器，详细请看system.cpp文件中的mySystemInit函数
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年10月19日
  ******************************************************************************
  */
/* USER CODE END Header */

#include "system/system.hpp"


//自定义全局变量
waypoint_msg waypoints;
trajectory_msg trajectory_message;



//自己创建的任务
/* Definitions for icm20602Task */
TaskHandle_t icm20602TaskHandle;
void Icm20602_main(void *argument);
SemaphoreHandle_t semIcm20602;

/* Definitions for magnetometerTask */
TaskHandle_t lsm303dTaskHandle;
void lsm303d_main(void *argument);
SemaphoreHandle_t semLsm303d;

/* Definitions for ahrsTask */
TaskHandle_t ahrsTaskHandle;
void ahrs_main(void *argument);
SemaphoreHandle_t semAhrs;

/* Definitions for ekfUwbTask */
TaskHandle_t ekfUwb1msTaskHandle;
TaskHandle_t ekfUwb5msTaskHandle;
void ekfUwb1ms_main(void *argument);
void ekfUwb5ms_main(void *argument);
SemaphoreHandle_t semEkfUwb1ms;
SemaphoreHandle_t semEkfUwb5ms;
SemaphoreHandle_t semEkfIns;

/* Definitions for tranTask */
TaskHandle_t tranTaskHandle;
void tran_main(void *argument);
SemaphoreHandle_t semTran;

/* Definitions for batteryTask */
TaskHandle_t batteryTaskHandle;
void battery_main(void *argument);
SemaphoreHandle_t semBattery;

/* Definitions for rcTask */
TaskHandle_t rcTaskHandle;
void rc_main(void *argument);

/* Definitions for motorTask */
TaskHandle_t motorTaskHandle;
void motor_main(void *argument);
SemaphoreHandle_t semMotor;

/* Definitions for attitudeCtrlTask */
TaskHandle_t attitudeCtrlTaskHandle;
void attitudeCtrl_main(void *argument);
SemaphoreHandle_t semAttitudeCtrl;

/* Definitions for geometricCtrlTask */
TaskHandle_t geometricCtrlTaskHandle;
void geometricCtrl_main(void *argument);
SemaphoreHandle_t semGeometricCtrl;

/* Definitions for geometricCtrlPosTask */
TaskHandle_t geometricCtrlPosTaskHandle;
void geometricCtrlPos_main(void *argument);
SemaphoreHandle_t semGeometricCtrlPos;

/* Definitions for positionCtrlTask */
TaskHandle_t positionCtrlTaskHandle;
void positionCtrl_main(void *argument);
SemaphoreHandle_t semPositionCtrl;

/* Definitions for tranReceiveTask */
TaskHandle_t tranReceiveTaskHandle;
void tranReceive_main(void *argument);

/* Definitions for flowTask */
TaskHandle_t flowTaskHandle;
void flow_main(void *argument);

/* Definitions for mahonyAhrsTask */
TaskHandle_t mahonyAhrsTaskHandle;
void mahonyAhrs_main(void *argument);
SemaphoreHandle_t semMahonyAhrs;

/* Definitions for ms5611Task */
TaskHandle_t ms5611TaskHandle;
void ms5611_main(void *argument);
SemaphoreHandle_t semMs5611;

/* Definitions for uwbTask */
TaskHandle_t uwbTaskHandle;
void uwb_main(void *argument);

//自己创建的任务
/* Definitions for ekfTask */
TaskHandle_t ekfTaskHandle;
void ekf_main(void *argument);
SemaphoreHandle_t semEkf;

/* Definitions for KalUwbTask */
TaskHandle_t kalUwbTaskHandle;
TaskHandle_t kalUwbPredTaskHandle;
void kalUwb_main(void *argument);
void kalUwb_pred(void *argument);
SemaphoreHandle_t semKalUwb;
SemaphoreHandle_t semKalUwbPred;

/* Definitions for trajectoryTask */
TaskHandle_t trajectoryTaskHandle;
void trajectory_main(void *argument);


/* Definitions for raspberrypiTask */
TaskHandle_t raspberrypiTaskHandle;
void raspberrypi_main(void *argument);
SemaphoreHandle_t semRaspberrypi;

/* Definitions for raspberrypiReceiveTask */
TaskHandle_t raspberrypiReceiveTaskHandle;
void raspberrypiReceive_main(void *argument);

/* Definitions for raspberrypiReceiveTask */
TaskHandle_t safecheckTaskHandle;
void safecheck_main(void *argument);
SemaphoreHandle_t semSafeCheck;

//自己创建的任务


QueueHandle_t queueGyrDat;			//陀螺角速度数据
QueueHandle_t queueGyrDatFil;		//角速度滤波后的数据
QueueHandle_t queueAccDat;			//加速度计数据
QueueHandle_t queueMagDat;			//磁力计数据
QueueHandle_t queueBaroAlt;			//气压计数据
QueueHandle_t queueAhrsEuler;		//姿态角
QueueHandle_t queueBattery;			//电池电压
QueueHandle_t queueRCData;			//遥控器PPM通道数据
QueueHandle_t queueRCCommand;		//遥控器解析后的指令
QueueHandle_t queueMotorPWM;		//电机、舵机输出
QueueHandle_t queuePID;				//PID参数
QueueHandle_t queueInloopControl;	//控制器的设定与反馈值，用于发送到上位机观察
QueueHandle_t queueFlow;			//光流数据
QueueHandle_t queueHeight;			//激光高度数据
QueueHandle_t queueMahonyAhrs;				//MahonyAhrs解算得到的位姿
QueueHandle_t queueUWB;
QueueHandle_t queueCb2n;
QueueHandle_t queueUwbPosVel;		//Uwb_EKF加速度融合得到的位置速度
QueueHandle_t queueMotorRps;		//电机转速
QueueHandle_t queueQuaternion;		//四元数
QueueHandle_t queueDeltaImu;		//增量角度和增量速度
QueueHandle_t queueDeltaImuToZeroFlag;		//增量角度和增量速度置零
QueueHandle_t queueEKF;		//EKF输出
QueueHandle_t queueAttitude;

QueueHandle_t queueControlMsg;
QueueHandle_t queueKalUwbPosVel;	//Uwb_KAL滤波得到的位置速度
///////////////////////////////////////////adding////////////////////////////////////////////////////

QueueHandle_t queueAddingR;
QueueHandle_t queueMoments;

/////////////////////////////////////////////////////////////////////////////////////////////////////
QueueHandle_t queueMOCAP;

TimerHandle_t myTimer1ms;


SYSTEM::SYSTEM()
{

}

SYSTEM::~SYSTEM()
{

}

/**********************************
 * function : 创建用户任务
 *
 * task list :	 任务句柄				任务功能					执行时间
 * 				icm20602Task		: 陀螺仪加速度计数据采集			31us
 * 				motorTask			: 电机驱动					24us
 * 				lsm303dTask			: 磁力计数据采集				15us
 * 				ahrsTask			: 姿态解算					195us
 * 				attitudeCtrlTask	: 内环控制（角度角速度串级PID控制）
 * 				tranTask			: 飞控数据发送到地面站
 * 				tranReceiveTask		: 地面站数据发送到飞控
 * 				batteryTask			: 电池电压采集
 * 				rcTask				: 遥控器
 *
 * para			:void
 *
 * return		:void
 *
 * *************************************/
void SYSTEM::taskCreat()
{
	isIcm20602TaskCreatSuccess = xTaskCreate ((TaskFunction_t)Icm20602_main, "icm20602Task", (uint16_t)(40 * 4), NULL, (osPriority_t) osPriorityRealtime5, &icm20602TaskHandle);
	isMotorTaskCreatSuccess = xTaskCreate ((TaskFunction_t)motor_main, "motorTask", (uint16_t)(40 * 4), NULL, (osPriority_t) osPriorityAboveNormal4, &motorTaskHandle);   //原RealTime6
	isMagTaskCreatSuccess = xTaskCreate ((TaskFunction_t)lsm303d_main, "lsm303dTask", (uint16_t)(40 * 4), NULL, (osPriority_t) osPriorityRealtime4, &lsm303dTaskHandle);
	isMs5611TaskCreatSuccess = xTaskCreate ((TaskFunction_t)ms5611_main, "ms5611Task", (uint16_t)(40 * 4), NULL, (osPriority_t) osPriorityHigh1, &ms5611TaskHandle);
	isAhrsTaskCreatSuccess = xTaskCreate ((TaskFunction_t)ahrs_main, "ahrsTask", (uint16_t)(256 * 4), NULL, (osPriority_t) osPriorityHigh7, &ahrsTaskHandle);
//	isMahonyAhrsTaskCreatSuccess = xTaskCreate ((TaskFunction_t)mahonyAhrs_main, "mahonyAhrsTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh7, &mahonyAhrsTaskHandle);
//	isEkfUwb1msTaskCreatSuccess = xTaskCreate ((TaskFunction_t)ekfUwb1ms_main, "ekfUwbTask1ms", (uint16_t)(50 * 4), NULL, (osPriority_t) osPriorityHigh4, &ekfUwb1msTaskHandle);
//	isEkfUwb5msTaskCreatSuccess = xTaskCreate ((TaskFunction_t)ekfUwb5ms_main, "ekfUwbTask5ms", (uint16_t)(512 * 4), NULL, (osPriority_t) osPriorityHigh4, &ekfUwb5msTaskHandle);
	isAttitudeCtrlTaskCreatSuccess = xTaskCreate ((TaskFunction_t)attitudeCtrl_main, "attitudeCtrlTask", (uint16_t)(160 * 4), NULL, (osPriority_t) osPriorityHigh6, &attitudeCtrlTaskHandle);
//	isGeometricCtrlTaskCreatSuccess = xTaskCreate ((TaskFunction_t)geometricCtrl_main, "geometricCtrlTask", (uint16_t)(40 * 4), NULL, (osPriority_t) osPriorityHigh6, &geometricCtrlTaskHandle);
//	isGeometricCtrlPosTaskCreatSuccess = xTaskCreate ((TaskFunction_t)geometricCtrlPos_main, "geometricCtrlPosTask", (uint16_t)(256 * 4), NULL, (osPriority_t) osPriorityHigh2, &geometricCtrlPosTaskHandle);
//	isPositionCtrlTaskCreatSuccess = xTaskCreate ((TaskFunction_t)positionCtrl_main, "positionCtrlTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh2, &positionCtrlTaskHandle);
	isTranTaskCreatSuccess = xTaskCreate ((TaskFunction_t)tran_main, "tranTask", (uint16_t)(120 * 4), NULL, (osPriority_t) osPriorityAboveNormal3, &tranTaskHandle);
//	isTranReceiveTaskCreatSuccess = xTaskCreate ((TaskFunction_t)tranReceive_main, "tranReceiveTask", (uint16_t)(60 * 4), NULL, (osPriority_t) osPriorityAboveNormal4, &tranReceiveTaskHandle);
//	isBatteryTaskCreatSuccess = xTaskCreate ((TaskFunction_t)battery_main, "batteryTask", (uint16_t)(50 * 4), NULL, (osPriority_t) osPriorityBelowNormal1, &batteryTaskHandle);
	isRcTaskCreatSuccess = xTaskCreate ((TaskFunction_t)rc_main, "rcTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityHigh3, &rcTaskHandle);
	isFlowTaskCreatSuccess = xTaskCreate ((TaskFunction_t)flow_main, "flowTask", (uint16_t)(60 * 4), NULL, (osPriority_t) osPriorityRealtime1, &flowTaskHandle);

	isKalUwbTaskCreatSuccess = xTaskCreate ((TaskFunction_t)kalUwb_main, "kalUwbTask", (uint16_t)(256 * 4), NULL, (osPriority_t) osPriorityHigh4, &kalUwbTaskHandle);
	isKalUwbPredTaskCreatSuccess = xTaskCreate ((TaskFunction_t)kalUwb_pred, "kalUwbPred", (uint16_t)(40 * 4), NULL, (osPriority_t) osPriorityHigh4, &kalUwbPredTaskHandle);
	isUwbTaskCreatSuccess = xTaskCreate ((TaskFunction_t)uwb_main, "uwbTask", (uint16_t)(60 * 4), NULL, (osPriority_t) osPriorityRealtime, &uwbTaskHandle);

	isSafeCheckTaskCreateSuccess = xTaskCreate((TaskFunction_t)safecheck_main, "safecheckTask", (uint16_t)(60 * 4), NULL, (osPriority_t) osPriorityRealtime2, &safecheckTaskHandle);

	/* 在这里添加你的任务 */
//	isEkfTaskCreatSuccess = xTaskCreate ((TaskFunction_t)ekf_main, "ekfTask", (uint16_t)(640 * 4), NULL, (osPriority_t) osPriorityHigh4, &ekfTaskHandle);
//	isTrajectoryTaskCreatSuccess = xTaskCreate ((TaskFunction_t)trajectory_main, "trajectoryTask", (uint16_t)(25000 * 4), NULL, (osPriority_t) osPriorityLow, &trajectoryTaskHandle);
//	isRaspberrypiTaskCreatSuccess = xTaskCreate ((TaskFunction_t)raspberrypi_main, "raspberrypiTask", (uint16_t)(128 * 4), NULL, (osPriority_t) osPriorityAboveNormal1, &raspberrypiTaskHandle);
	isRaspberrypiReceiveTaskCreatSuccess = xTaskCreate ((TaskFunction_t)raspberrypiReceive_main, "raspberrypiReceiveTask", (uint16_t)(40 * 4), NULL, (osPriority_t) osPriorityAboveNormal2, &raspberrypiReceiveTaskHandle);

	/* 在这里添加你的任务 */

}

void SYSTEM::semCreat()
{
	semIcm20602 = xSemaphoreCreateBinary();
	semLsm303d = xSemaphoreCreateBinary();
	semMs5611 = xSemaphoreCreateBinary();
	semAhrs = xSemaphoreCreateBinary();
	semMahonyAhrs = xSemaphoreCreateBinary();
	semEkfUwb1ms = xSemaphoreCreateBinary();
	semEkfUwb5ms = xSemaphoreCreateBinary();

	semEkfIns = xSemaphoreCreateBinary();

	semKalUwb = xSemaphoreCreateBinary();
	semKalUwbPred = xSemaphoreCreateBinary();

	semAttitudeCtrl = xSemaphoreCreateBinary();
	semGeometricCtrl = xSemaphoreCreateBinary();
	semGeometricCtrlPos = xSemaphoreCreateBinary();
	semPositionCtrl = xSemaphoreCreateBinary();
	semMotor = xSemaphoreCreateBinary();
	semTran = xSemaphoreCreateBinary();
	semBattery = xSemaphoreCreateBinary();
	/* 在这里添加你的信号量 */

	semEkf = xSemaphoreCreateBinary();
	semRaspberrypi = xSemaphoreCreateBinary();

	semSafeCheck = xSemaphoreCreateBinary();

	/* 在这里添加你的信号量 */
}

void SYSTEM::timerCreat()
{
	myTimer1ms = xTimerCreate ("myTimer", 1, pdTRUE, (void *)0, (TimerCallbackFunction_t)Timer1msCallback);
	xTimerStart(myTimer1ms,0);
}

void SYSTEM::queueCreat()
{
	queueGyrDat = xQueueCreate(1,sizeof(sensor_gyro_msg));
	queueGyrDatFil = xQueueCreate(1,sizeof(sensor_gyro_filter_msg));
	queueAccDat = xQueueCreate(1,sizeof(sensor_acc_msg));
	queueMagDat = xQueueCreate(1,sizeof(sensor_mag_msg));
	queueBaroAlt = xQueueCreate(1,sizeof(sensor_baroAlt_msg));
	queueAhrsEuler = xQueueCreate(1,sizeof(ahrs_euler_msg));
	queueBattery = xQueueCreate(1,sizeof(battery_msg));
	queueRCData = xQueueCreate(1,sizeof(RCData));
	queueRCCommand = xQueueCreate(1,sizeof(RC_command_msg));
	queueMotorPWM = xQueueCreate(1,sizeof(Motor_PWM_msg));
	queuePID = xQueueCreate(1,sizeof(Pid_msg));
	queueInloopControl = xQueueCreate(1,sizeof(Inloop_control_msg));
	queueFlow = xQueueCreate(1,sizeof(flow_msg));
	queueHeight = xQueueCreate(1,sizeof(height_msg));
	queueMahonyAhrs = xQueueCreate(1,sizeof(mahonyAhrs_msg));;
	queueUWB = xQueueCreate(1,sizeof(uwb_pos_msg));
	queueCb2n = xQueueCreate(1,sizeof(Cb2n_msg));
	queueUwbPosVel = xQueueCreate(1,sizeof(ekf_uwb_pos_vel_msg));
	queueKalUwbPosVel = xQueueCreate(1,sizeof(kal_uwb_pos_vel_msg));

	queueMotorRps = xQueueCreate(1,sizeof(motor_rps_msg));
	queueQuaternion = xQueueCreate(1,sizeof(q_msg));
	/* 在这里添加你的消息队列 */
	queueDeltaImu = xQueueCreate(1,sizeof(delta_angle_and_vel_msg));
	queueDeltaImuToZeroFlag = xQueueCreate(1,sizeof(delta_to_zeros_flag_msg));
	queueEKF = xQueueCreate(1,sizeof(ekf_msg));
	queueControlMsg = xQueueCreate(1,sizeof(Control_msg));
	/* 在这里添加你的消息队列 */

//////////////////////////////////////adding////////////////////////////////////////////////

	queueAddingR = xQueueCreate(1,sizeof(addingR));
	queueMoments = xQueueCreate(1,sizeof(addingMoments));

////////////////////////////////////////////////////////////////////////////////////////////

	queueMOCAP = xQueueCreate(1,sizeof(mocap_pos_msg));

}

SYSTEM system0;
/* 在启动实时操作系统之前调用该函数 */
void mySystemInit(void)
{
	system0.taskCreat();				//任务的创建
	system0.semCreat();				//信号量的创建,用于任务的同步
//	system0.timerCreat();			//软件定时器的创建,用于任务的时间片调度
	system0.queueCreat();			//消息队列的创建,用于任务之间的通信

}
void TIM5_IRQHandler(void)
{
	if(LL_TIM_IsActiveFlag_UPDATE(TIM5) != RESET)
	{
		LL_TIM_ClearFlag_UPDATE(TIM5);

		system0.startTimerLast = system0.startTimer;
		getTimer_us(&system0.startTimer);
		system0.cycleTime_us = system0.startTimer - system0.startTimerLast;
		//任务时间片调度,设定定时任务时间片长度,单位为ms
		static uint32_t _cnt = 0;
		const uint32_t taskPeriod[20] = {1,5,10,20,200};
		if(_cnt%taskPeriod[0] == taskPeriod[0]-1)
		{
			osSemaphoreRelease(semIcm20602);
			osSemaphoreRelease(semAhrs);
	//		osSemaphoreRelease(semMahonyAhrs);
			osSemaphoreRelease(semAttitudeCtrl);
//			osSemaphoreRelease(semGeometricCtrl);
			osSemaphoreRelease(semMotor);
//			osSemaphoreRelease(semEkfUwb1ms);
			osSemaphoreRelease(semKalUwbPred);
		}
		if(_cnt%taskPeriod[1] == taskPeriod[1]-1)
		{
	//		osSemaphoreRelease(semPositionCtrl);
	//		osSemaphoreRelease(semEkf);
	//		osSemaphoreRelease(semGeometricCtrlPos);
//			osSemaphoreRelease(semEkfUwb5ms);
			osSemaphoreRelease(semKalUwb);
		}
		if(_cnt%taskPeriod[2] == taskPeriod[2]-1)
		{
			osSemaphoreRelease(semTran);
			osSemaphoreRelease(semMs5611);
		}

		if(_cnt%taskPeriod[3] == taskPeriod[3]-1)
		{
			osSemaphoreRelease(semLsm303d);
			osSemaphoreRelease(semSafeCheck);
		}
		if(_cnt%taskPeriod[4] == taskPeriod[4]-1)
		{
			osSemaphoreRelease(semBattery);
//			osSemaphoreRelease(semRaspberrypi);
		}
		_cnt++;

		getTimer_us(&system0.stopTimer);

		system0.executionTime_us = system0.stopTimer - system0.startTimer;
	}
}
//void Timer1msCallback(void *argument)
//{
//	system0.startTimerLast = system0.startTimer;
//	getTimer_us(&system0.startTimer);
//	system0.cycleTime_us = system0.startTimer - system0.startTimerLast;
//	//任务时间片调度,设定定时任务时间片长度,单位为ms
//	static uint32_t _cnt = 0;
//	const uint32_t taskPeriod[20] = {1,5,10,20,200};
//	if(_cnt%taskPeriod[0] == taskPeriod[0]-1)
//	{
//		osSemaphoreRelease(semIcm20602);
//		osSemaphoreRelease(semAhrs);
////		osSemaphoreRelease(semMahonyAhrs);
////		osSemaphoreRelease(semAttitudeCtrl);
//		osSemaphoreRelease(semGeometricCtrl);
//		osSemaphoreRelease(semMotor);
//	}
//	if(_cnt%taskPeriod[1] == taskPeriod[1]-1)
//	{
////		osSemaphoreRelease(semPositionCtrl);
//		osSemaphoreRelease(semEkfUwb);
////		osSemaphoreRelease(semEkf);
//		osSemaphoreRelease(semGeometricCtrlPos);
//	}
//	if(_cnt%taskPeriod[2] == taskPeriod[2]-1)
//	{
//		osSemaphoreRelease(semTran);
//		osSemaphoreRelease(semMs5611);
//	}
//
//	if(_cnt%taskPeriod[3] == taskPeriod[3]-1)
//	{
//		osSemaphoreRelease(semLsm303d);
//	}
//	if(_cnt%taskPeriod[4] == taskPeriod[4]-1)
//	{
//		osSemaphoreRelease(semBattery);
//	}
//	_cnt++;
//
//	getTimer_us(&system0.stopTimer);
//
//	system0.executionTime_us = system0.stopTimer - system0.startTimer;
//
//}


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
